
#include "chassis.h"
#include "robot_def.h"

#include "general_def.h"
#include "bsp_dwt.h"
#include "arm_math.h"
#include "bsp_pwm.h"
#include "PWM_motor.h"
#include "tim.h"
#include "mpu6500.h"
#include "message_center.h"
#include "dji_motor.h"
#include "servo_motor.h"
static PWM_MOTOR_Instance_s *PWM_Motor_R, *PWM_Motor_L;
static DJIMotorInstance *m2006;

static attitude_t *gimba_IMU_data; // 云台IMU数据

static Publisher_t *chassis_pub;  // 用于发布底盘的数据
static Subscriber_t *chassis_sub; // 用于订阅底盘的控制命令

static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // 底盘接收到的控制命令

#define ChassisWidth 12.0f
#define ChassisRadius (ChassisWidth / 2f)
#define WheelRadius 3.25f
void ChassisInit()
{
    HAL_GPIO_WritePin(GPIOG, D8_Pin | D7_Pin | D6_Pin | D5_Pin | D4_Pin | D3_Pin | D2_Pin | D1_Pin, GPIO_PIN_SET);
    gimba_IMU_data = INS_Init();
    PWM_MOTOR_Init_Config_s PWM_Motor_L_Config = {
        .pwm_init_config = {
            .channel = TIM_CHANNEL_1,
            .dutyratio = 0.5,
            .htim = &htim2,
            .period = 1,
        },
        .dir_gpio_1 = {
            .gpio_port = GPIOC,
            .gpio_pin = GPIO_PIN_2,
        },
        .dir_gpio_2 = {
            .gpio_port = GPIOC,
            .gpio_pin = GPIO_PIN_3,
        },
        .close_loop_type = ANGLE_LOOP,
        .controller_param_init_config = {
            .angle_PID = {
                .Kp = 0.75,
                .Ki = 0,
                .Kd = 0.015,
                .MaxOut = 100,
            },
            .speed_PID = {
                .Kp = 10,
                .Ki = 0,
                .Kd = 0,
                .MaxOut = 100,
            },
            .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
        },
        .angle_feedback_source = MOTOR_FEED,
        .control_type = PWM_MOTOR_CLOSELOOP,
        .direction = PWM_MOTOR_DIRECTION_NORMAL,
        .encoder_tim = &htim8,
        .angle_feedback_dir = FEEDBACK_DIRECTION_NORMAL,
        .speed_feedback_dir = FEEDBACK_DIRECTION_NORMAL,
    };
    PWM_Motor_L = PWMMotorInit(&PWM_Motor_L_Config);
    PWM_MOTOR_Init_Config_s PWM_Motor_R_Config = {
        .pwm_init_config = {
            .channel = TIM_CHANNEL_1,
            .dutyratio = 0.5,
            .htim = &htim4,
            .period = 1,
        },
        .dir_gpio_1 = {
            .gpio_port = GPIOE,
            .gpio_pin = GPIO_PIN_5,
        },
        .dir_gpio_2 = {
            .gpio_port = GPIOE,
            .gpio_pin = GPIO_PIN_6,
        },
        .close_loop_type = ANGLE_LOOP,
        .controller_param_init_config = {
            .angle_PID = {
                .Kp = 0.75,
                .Ki = 0,
                .Kd = 0.015,
                .MaxOut = 100,
            },
            .speed_PID = {
                .Kp = 10,
                .Ki = 0,
                .Kd = 0,
                .MaxOut = 100,
            },
            .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
        },
        .control_type = PWM_MOTOR_CLOSELOOP,
        .direction = PWM_MOTOR_DIRECTION_REVERSE,
        .encoder_tim = &htim5,
        .angle_feedback_source = MOTOR_FEED,
        .angle_feedback_dir = FEEDBACK_DIRECTION_NORMAL,
        .speed_feedback_dir = FEEDBACK_DIRECTION_NORMAL,
    };
    PWM_Motor_R = PWMMotorInit(&PWM_Motor_R_Config);
    Motor_Init_Config_s m2006_config = {
        .can_init_config  = {
            .can_handle = &hcan1,
            .tx_id = 1
        },
        .motor_type = M2006,
        .controller_param_init_config = {
            .angle_PID = {
                .Kp = 0,
                .Ki = 0,
                .Kd = 0,
                .MaxOut = 100,
            },
            .speed_PID ={
                .Kp = 1,
                .Ki = 0,
                .Kd = 0,
                .MaxOut = 2000,
            },
            },
            .controller_setting_init_config={
                .close_loop_type = SPEED_LOOP,
                .outer_loop_type = SPEED_LOOP,
                .speed_feedback_source = MOTOR_FEED,
                .angle_feedback_source = MOTOR_FEED,
            }
        };

    m2006 = DJIMotorInit(&m2006_config);
    chassis_sub = SubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s));
}

// 陀螺仪顺时针为负
void ChassisTask()
{
    DJIMotorSetRef(m2006, 2000);
    SubGetMessage(chassis_sub, &chassis_cmd_recv);

    switch (chassis_cmd_recv.chassis_mode)
    {
    case CHASSIS_STOP:
        PWMMotorSetWorkingState(PWM_Motor_R, PWM_MOTOR_OFF);
        PWMMotorSetWorkingState(PWM_Motor_L, PWM_MOTOR_OFF);

        break;
    case CHASSIS_Straight:
        PWMMotorSetWorkingState(PWM_Motor_R, PWM_MOTOR_ON);
        PWMMotorSetWorkingState(PWM_Motor_L, PWM_MOTOR_ON);

        PWMMotorSetControLoopType(PWM_Motor_R, PWM_MOTOR_OPENLOOP);
        PWMMotorSetControLoopType(PWM_Motor_L, PWM_MOTOR_OPENLOOP);

        PWMMotorSetDirection(PWM_Motor_R, PWM_MOTOR_DIRECTION_REVERSE);
        PWMMotorSetDirection(PWM_Motor_L, PWM_MOTOR_DIRECTION_NORMAL);

        PWMMotorSetSpeed(PWM_Motor_R, chassis_cmd_recv.speed);
        PWMMotorSetSpeed(PWM_Motor_L, chassis_cmd_recv.speed);

        break;
    case CHASSIS_Transverse:
        PWMMotorSetWorkingState(PWM_Motor_R, PWM_MOTOR_ON);
        PWMMotorSetWorkingState(PWM_Motor_L, PWM_MOTOR_ON);

        PWMMotorSetControLoopType(PWM_Motor_R, PWM_MOTOR_CLOSELOOP);
        PWMMotorSetControLoopType(PWM_Motor_L, PWM_MOTOR_CLOSELOOP);

        PWMMotorSetClossingLoopType(PWM_Motor_R, ANGLE_AND_SPEED_LOOP);
        PWMMotorSetClossingLoopType(PWM_Motor_L, ANGLE_AND_SPEED_LOOP);

        PWMMotorSetRef(PWM_Motor_R, chassis_cmd_recv.speed);
        PWMMotorSetRef(PWM_Motor_L, -chassis_cmd_recv.speed);
        break;
    default:
        PWMMotorSetWorkingState(PWM_Motor_R, PWM_MOTOR_OFF);
        PWMMotorSetWorkingState(PWM_Motor_L, PWM_MOTOR_OFF);

        break;
    }
}